#ifndef ROBOTAIMER_H
#define ROBOTAIMER_H

#include "WPILib.h"
#include "CameraLights.h"
#include "TankDrive.h"

//------------------------------------------------------------------------------
// DEFINE RobotAimer CLASS
//------------------------------------------------------------------------------
//
//------------------------------------------------------------------------------
class RobotAimer
{
	public:
		static const bool kOff = false;
		static const bool kOn  = true;

		static const UINT32 DIGITAL_SIDECAR_MODULE =   1;
		static const UINT32 DISK_AIMER_CH		   =   8;
		static const UINT32 ANGLE_SENSOR_CH		   =   6;

		RobotAimer(TankDrive *pTankDrive);
		~RobotAimer();
		bool ProcessImage();
		bool AimRobotX();
		bool AimRobotY();
		void AdjustShooterAngle(float target);
		void AutoAdjustShooterAngle(float speed);
		void SetInputToOutputFormula(float AIMER_CONST, float AIMER_RATIO);

		bool  robotAimedY;
		bool  robotAimedX;
		bool  targetCaptured;
		bool  targetLock;

	private:
		//AxisCamera *pCamera;
		ParticleAnalysisReport *highestReport;
		Victor 		 		   *pDiskAimer;
		CameraLights 		   *pProcessorLights;
		DigitalInput 		   *pAngleSensor;
		TankDrive	 		   *pTurningWheels;

		static const float STOP_MOTOR			   =   0;
		static const float TURN_SPEED			   =  .2;
		static const float REVERSE_SPEED		   = -.2;
		static const int   GRANNY_GEAR			   =   0;
		static const float ANGLE_REVERSE_SPEED     = -.2;
		static const float ANGLE_SPEED			   =  .2;

		float	CONST;
		float   RATIO;

		//Aiming Variables
		int sensitivityX;
		int sensitivityY;
		int upperBoundX;
		int lowerBoundX;
		int upperBoundY;
		int lowerBoundY;
		int actualX;
		int actualY;
		bool angleSensorReading;
		bool lastAngleSensorReading;
		int shooterAngle;

		// Turn motor speed
		double turnSpeed;
		double raiseSpeed;

};
#endif
